/*
 * TCS34725.c
 *
 *  Created on: Oct 11, 2024
 *      Author: 26090
 */
#include "TCS34725.h"
#include "math.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "robot_def.h"

static TCS3472Instance *TCS3472;

float r_ratio;
float g_ratio;
float b_ratio;

#define equal_ratio(a,b) ((a-b)>0?(a-b)<0.04f:(a-b)>-0.04f)
#define great_ratio(a,b) ((a-b)>0.07f)
#define max_ratio(a,b) (a>b?a:b)

// 颜色识别函数
COLOR_e identify_color()
{
	if (TCS3472->c == 0) {
		return COLOR_UNKNOWN; // 防止除零错误
	}
	if(TCS3472->c > (95 + 70))
	{
		r_ratio = (float)TCS3472->r / (float)TCS3472->c;
		g_ratio = (float)TCS3472->g / (float)TCS3472->c;
		b_ratio = (float)TCS3472->b / (float)TCS3472->c;
		if (great_ratio(r_ratio, g_ratio) && great_ratio(r_ratio, b_ratio) && (equal_ratio(g_ratio,b_ratio)))
		{	return COLOR_RED;}
		else if (great_ratio(r_ratio, g_ratio) && great_ratio(g_ratio, b_ratio))
		{	return COLOR_YELLOW;}
		else if (great_ratio(b_ratio, g_ratio) && great_ratio(g_ratio, r_ratio))
		{	return COLOR_BLUE;}
		else if ((equal_ratio(g_ratio, b_ratio)) && (max_ratio(g_ratio, b_ratio) - r_ratio) > 0.08f)
		{	return COLOR_BLACK;}
		else
		{	return COLOR_UNKNOWN;}
	}
	else 
	{
		return COLOR_UNKNOWN;
	}

}

/**
 * @描述:基于TCS34725 写寄存器 ID-地址-数据
 */
void TCS34725_WriteReg(uint8_t reg,uint8_t data)
{
	IICAccessMem(TCS3472->iic, TCS34725_COMMAND_BIT | reg, &data, 1, IIC_WRITE_MEM, 1);
}

/**
 * @描述:基于TCS34725 读寄存器 ID-地址-数据
 */
uint8_t TCS34725_ReadReg(uint8_t reg)
{
	uint8_t reData = 0;

	IICAccessMem(TCS3472->iic, TCS34725_COMMAND_BIT | reg, &reData, 1, IIC_WRITE_MEM, 1);
  	return reData;
}

/**
* @描述:初始化TCS34725配置
*/
TCS3472Instance* TCS34725_Init(I2C_HandleTypeDef* hi2c)
{
	TCS3472 = (TCS3472Instance *)malloc(sizeof(TCS3472Instance));
	IIC_Init_Config_s conf = {
		.handle = hi2c,
		.dev_address = TCS34725_ADDRESS_7bit,
		.id = TCS3472,
		.work_mode = IIC_BLOCK_MODE,
	};
	TCS3472->iic = IICRegister(&conf);
	DWT_Delay(2);
	TCS34725_setIntegrationTime(TCS34725_INTEGRATIONTIME_101MS);
	TCS34725_setGain(TCS34725_GAIN_1X);
	TCS34725_enable();
	DWT_Delay(4);
	return TCS3472;
}

/**
* @描述:设置时间增益
*/
void TCS34725_setIntegrationTime(tcs34725IntegrationTime_t time)
{
  // 更新时序寄存器
	TCS34725_WriteReg(TCS34725_ATIME,time);
}

/**
* @描述:设置增益
*/
void TCS34725_setGain(tcs34725Gain_t gain)
{
	// 设置增益
	TCS34725_WriteReg(TCS34725_CONTROL,gain);

}

/**
* @描述:使能器件
*/
void TCS34725_enable(void)
{
	// 开启内部振荡器--启动
	TCS34725_WriteReg(TCS34725_ENABLE,TCS34725_ENABLE_PON);
 	DWT_Delay(1);
	// 启动ADC
	TCS34725_WriteReg(TCS34725_ENABLE,TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN);
}

/**
* @描述:读取TCS34725指定寄存器
*/
uint16_t TCS34725_readRegWord(uint8_t reg)
{
	uint8_t buf[2];
  uint16_t h = 0x0000;		// 高八位
	uint16_t l = 0x0000;		// 低八位

	IICAccessMem(TCS3472->iic, TCS34725_COMMAND_BIT | reg | 0x20, buf, 2, IIC_READ_MEM, 1);
	h=buf[1];
	l=buf[0];
	h <<= 8;
	h |= l;
	return h;
}

/**
* @描述:获取TCS34725的颜色反馈
*/
void TCS34725_getRGBC()
{

	TCS3472->c = TCS34725_readRegWord(TCS34725_CDATAL);
  	TCS3472->r = TCS34725_readRegWord(TCS34725_RDATAL);
  	TCS3472->g = TCS34725_readRegWord(TCS34725_GDATAL);
  	TCS3472->b = TCS34725_readRegWord(TCS34725_BDATAL);

  	TCS3472->color_TCS34725 = identify_color();
}

/**
* @描述:使能TCS34725内部中断
*/
void TCS34725_lock()
{
	uint8_t r = TCS34725_ReadReg(TCS34725_ENABLE);
	r |= TCS34725_ENABLE_AIEN;
	TCS34725_WriteReg(TCS34725_ENABLE, r);
}

/**
* @描述:获取器件ID
* @返回:0x44 = TCS34721/TCS34725, 0x4D = TCS34723/TCS34727
*/
uint8_t TCS34725_getID()
{
	return TCS34725_ReadReg(TCS34725_ID);
}

/**
* @描述:获取TCS34725状态
* @返回:返回该寄存器数值
*/
uint8_t TCS34725_getStatus()
{
	return TCS34725_ReadReg(TCS34725_STATUS);
}


